/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/

This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:

1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
///btSoftBody implementation by Nathanael Presson

#ifndef _BT_SOFT_BODY_INTERNALS_H
#define _BT_SOFT_BODY_INTERNALS_H

#include "btSoftBody.h"

#include "btQuickprof.h"
#include "btBroadphaseInterface.h"
#include "btCollisionDispatcher.h"
#include "btConvexInternalShape.h"
#include "btGjkEpa2.h"

//
// btSymMatrix
//
template <typename T>
struct btSymMatrix
{
    btSymMatrix() : dim(0)                    {}
    btSymMatrix(int n,const T& init=T())    { resize(n,init); }
    void                    resize(int n,const T& init=T())            { dim=n;store.resize((n*(n+1))/2,init); }
    int                        index(int c,int r) const                { if(c>r) btSwap(c,r);btAssert(r<dim);return((r*(r+1))/2+c); }
    T&                        operator()(int c,int r)                    { return(store[index(c,r)]); }
    const T&                operator()(int c,int r) const            { return(store[index(c,r)]); }
    btAlignedObjectArray<T>    store;
    int                        dim;
};

//
// btSoftBodyCollisionShape
//
class btSoftBodyCollisionShape : public btConcaveShape
{
public:
    btSoftBody*                        m_body;

    btSoftBodyCollisionShape(btSoftBody* backptr)
    {
        m_shapeType = SOFTBODY_SHAPE_PROXYTYPE;
        m_body=backptr;
    }

    virtual ~btSoftBodyCollisionShape()
    {

    }

    void    processAllTriangles(btTriangleCallback* /*callback*/,const btVector3& /*aabbMin*/,const btVector3& /*aabbMax*/) const
    {
        //not yet
        btAssert(0);
    }

    ///getAabb returns the axis aligned bounding box in the coordinate frame of the given transform t.
    virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const
    {
        /* t should be identity, but better be safe than...fast? */
        const btVector3    mins=m_body->m_bounds[0];
        const btVector3    maxs=m_body->m_bounds[1];
        const btVector3    crns[]={t*btVector3(mins.x(),mins.y(),mins.z()),
            t*btVector3(maxs.x(),mins.y(),mins.z()),
            t*btVector3(maxs.x(),maxs.y(),mins.z()),
            t*btVector3(mins.x(),maxs.y(),mins.z()),
            t*btVector3(mins.x(),mins.y(),maxs.z()),
            t*btVector3(maxs.x(),mins.y(),maxs.z()),
            t*btVector3(maxs.x(),maxs.y(),maxs.z()),
            t*btVector3(mins.x(),maxs.y(),maxs.z())};
        aabbMin=aabbMax=crns[0];
        for(int i=1;i<8;++i)
        {
            aabbMin.setMin(crns[i]);
            aabbMax.setMax(crns[i]);
        }
    }


    virtual void    setLocalScaling(const btVector3& /*scaling*/)
    {
        ///na
    }
    virtual const btVector3& getLocalScaling() const
    {
        static const btVector3 dummy(1,1,1);
        return dummy;
    }
    virtual void    calculateLocalInertia(btScalar /*mass*/,btVector3& /*inertia*/) const
    {
        ///not yet
        btAssert(0);
    }
    virtual const char*    getName()const
    {
        return "SoftBody";
    }

};

//
// btSoftClusterCollisionShape
//
class btSoftClusterCollisionShape : public btConvexInternalShape
{
public:
    const btSoftBody::Cluster*    m_cluster;

    btSoftClusterCollisionShape (const btSoftBody::Cluster* cluster) : m_cluster(cluster) { setMargin(0); }


    virtual btVector3    localGetSupportingVertex(const btVector3& vec) const
    {
        btSoftBody::Node* const *                        n=&m_cluster->m_nodes[0];
        btScalar                                        d=dot(vec,n[0]->m_x);
        int                                                j=0;
        for(int i=1,ni=m_cluster->m_nodes.size();i<ni;++i)
        {
            const btScalar    k=dot(vec,n[i]->m_x);
            if(k>d) { d=k;j=i; }
        }
        return(n[j]->m_x);
    }
    virtual btVector3    localGetSupportingVertexWithoutMargin(const btVector3& vec)const
    {
        return(localGetSupportingVertex(vec));
    }
    //notice that the vectors should be unit length
    virtual void    batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const
    {}


    virtual void    calculateLocalInertia(btScalar mass,btVector3& inertia) const
    {}

    virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const
    {}

    virtual int    getShapeType() const { return SOFTBODY_SHAPE_PROXYTYPE; }

    //debugging
    virtual const char*    getName()const {return "SOFTCLUSTER";}

    virtual void    setMargin(btScalar margin)
    {
        btConvexInternalShape::setMargin(margin);
    }
    virtual btScalar    getMargin() const
    {
        return getMargin();
    }
};

//
// Inline's
//

//
template <typename T>
static inline void            ZeroInitialize(T& value)
{
    static const T    zerodummy;
    value=zerodummy;
}
//
template <typename T>
static inline bool            CompLess(const T& a,const T& b)
{ return(a<b); }
//
template <typename T>
static inline bool            CompGreater(const T& a,const T& b)
{ return(a>b); }
//
template <typename T>
static inline T                Lerp(const T& a,const T& b,btScalar t)
{ return(a+(b-a)*t); }
//
template <typename T>
static inline T                InvLerp(const T& a,const T& b,btScalar t)
{ return((b+a*t-b*t)/(a*b)); }
//
static inline btMatrix3x3    Lerp(    const btMatrix3x3& a,
                                 const btMatrix3x3& b,
                                 btScalar t)
{
    btMatrix3x3    r;
    r[0]=Lerp(a[0],b[0],t);
    r[1]=Lerp(a[1],b[1],t);
    r[2]=Lerp(a[2],b[2],t);
    return(r);
}
//
static inline btVector3        Clamp(const btVector3& v,btScalar maxlength)
{
    const btScalar sql=v.length2();
    if(sql>(maxlength*maxlength))
        return((v*maxlength)/btSqrt(sql));
    else
        return(v);
}
//
template <typename T>
static inline T                Clamp(const T& x,const T& l,const T& h)
{ return(x<l?l:x>h?h:x); }
//
template <typename T>
static inline T                Sq(const T& x)
{ return(x*x); }
//
template <typename T>
static inline T                Cube(const T& x)
{ return(x*x*x); }
//
template <typename T>
static inline T                Sign(const T& x)
{ return((T)(x<0?-1:+1)); }
//
template <typename T>
static inline bool            SameSign(const T& x,const T& y)
{ return((x*y)>0); }
//
static inline btScalar        ClusterMetric(const btVector3& x,const btVector3& y)
{
    const btVector3    d=x-y;
    return(btFabs(d[0])+btFabs(d[1])+btFabs(d[2]));
}
//
static inline btMatrix3x3    ScaleAlongAxis(const btVector3& a,btScalar s)
{
    const btScalar    xx=a.x()*a.x();
    const btScalar    yy=a.y()*a.y();
    const btScalar    zz=a.z()*a.z();
    const btScalar    xy=a.x()*a.y();
    const btScalar    yz=a.y()*a.z();
    const btScalar    zx=a.z()*a.x();
    btMatrix3x3        m;
    m[0]=btVector3(1-xx+xx*s,xy*s-xy,zx*s-zx);
    m[1]=btVector3(xy*s-xy,1-yy+yy*s,yz*s-yz);
    m[2]=btVector3(zx*s-zx,yz*s-yz,1-zz+zz*s);
    return(m);
}
//
static inline btMatrix3x3    Cross(const btVector3& v)
{
    btMatrix3x3    m;
    m[0]=btVector3(0,-v.z(),+v.y());
    m[1]=btVector3(+v.z(),0,-v.x());
    m[2]=btVector3(-v.y(),+v.x(),0);
    return(m);
}
//
static inline btMatrix3x3    Diagonal(btScalar x)
{
    btMatrix3x3    m;
    m[0]=btVector3(x,0,0);
    m[1]=btVector3(0,x,0);
    m[2]=btVector3(0,0,x);
    return(m);
}
//
static inline btMatrix3x3    Add(const btMatrix3x3& a,
                                const btMatrix3x3& b)
{
    btMatrix3x3    r;
    for(int i=0;i<3;++i) r[i]=a[i]+b[i];
    return(r);
}
//
static inline btMatrix3x3    Sub(const btMatrix3x3& a,
                                const btMatrix3x3& b)
{
    btMatrix3x3    r;
    for(int i=0;i<3;++i) r[i]=a[i]-b[i];
    return(r);
}
//
static inline btMatrix3x3    Mul(const btMatrix3x3& a,
                                btScalar b)
{
    btMatrix3x3    r;
    for(int i=0;i<3;++i) r[i]=a[i]*b;
    return(r);
}
//
static inline void            Orthogonalize(btMatrix3x3& m)
{
    m[2]=cross(m[0],m[1]).normalized();
    m[1]=cross(m[2],m[0]).normalized();
    m[0]=cross(m[1],m[2]).normalized();
}
//
static inline btMatrix3x3    MassMatrix(btScalar im,const btMatrix3x3& iwi,const btVector3& r)
{
    const btMatrix3x3    cr=Cross(r);
    return(Sub(Diagonal(im),cr*iwi*cr));
}

//
static inline btMatrix3x3    ImpulseMatrix(    btScalar dt,
                                          btScalar ima,
                                          btScalar imb,
                                          const btMatrix3x3& iwi,
                                          const btVector3& r)
{
    return(Diagonal(1/dt)*Add(Diagonal(ima),MassMatrix(imb,iwi,r)).inverse());
}

//
static inline btMatrix3x3    ImpulseMatrix(    btScalar ima,const btMatrix3x3& iia,const btVector3& ra,
                                          btScalar imb,const btMatrix3x3& iib,const btVector3& rb)
{
    return(Add(MassMatrix(ima,iia,ra),MassMatrix(imb,iib,rb)).inverse());
}

//
static inline btMatrix3x3    AngularImpulseMatrix(    const btMatrix3x3& iia,
                                                 const btMatrix3x3& iib)
{
    return(Add(iia,iib).inverse());
}

//
static inline btVector3        ProjectOnAxis(    const btVector3& v,
                                          const btVector3& a)
{
    return(a*dot(v,a));
}
//
static inline btVector3        ProjectOnPlane(    const btVector3& v,
                                           const btVector3& a)
{
    return(v-ProjectOnAxis(v,a));
}

//
static inline void            ProjectOrigin(    const btVector3& a,
                                          const btVector3& b,
                                          btVector3& prj,
                                          btScalar& sqd)
{
    const btVector3    d=b-a;
    const btScalar    m2=d.length2();
    if(m2>SIMD_EPSILON)
    {
        const btScalar    t=Clamp<btScalar>(-dot(a,d)/m2,0,1);
        const btVector3    p=a+d*t;
        const btScalar    l2=p.length2();
        if(l2<sqd)
        {
            prj=p;
            sqd=l2;
        }
    }
}
//
static inline void            ProjectOrigin(    const btVector3& a,
                                          const btVector3& b,
                                          const btVector3& c,
                                          btVector3& prj,
                                          btScalar& sqd)
{
    const btVector3&    q=cross(b-a,c-a);
    const btScalar        m2=q.length2();
    if(m2>SIMD_EPSILON)
    {
        const btVector3    n=q/btSqrt(m2);
        const btScalar    k=dot(a,n);
        const btScalar    k2=k*k;
        if(k2<sqd)
        {
            const btVector3    p=n*k;
            if(    (dot(cross(a-p,b-p),q)>0)&&
                (dot(cross(b-p,c-p),q)>0)&&
                (dot(cross(c-p,a-p),q)>0))
            {
                prj=p;
                sqd=k2;
            }
            else
            {
                ProjectOrigin(a,b,prj,sqd);
                ProjectOrigin(b,c,prj,sqd);
                ProjectOrigin(c,a,prj,sqd);
            }
        }
    }
}

//
template <typename T>
static inline T                BaryEval(        const T& a,
                                     const T& b,
                                     const T& c,
                                     const btVector3& coord)
{
    return(a*coord.x()+b*coord.y()+c*coord.z());
}
//
static inline btVector3        BaryCoord(    const btVector3& a,
                                      const btVector3& b,
                                      const btVector3& c,
                                      const btVector3& p)
{
    const btScalar    w[]={    cross(a-p,b-p).length(),
        cross(b-p,c-p).length(),
        cross(c-p,a-p).length()};
    const btScalar    isum=1/(w[0]+w[1]+w[2]);
    return(btVector3(w[1]*isum,w[2]*isum,w[0]*isum));
}

//
static btScalar                ImplicitSolve(    btSoftBody::ImplicitFn* fn,
                                          const btVector3& a,
                                          const btVector3& b,
                                          const btScalar accuracy,
                                          const int maxiterations=256)
{
    btScalar    span[2]={0,1};
    btScalar    values[2]={fn->Eval(a),fn->Eval(b)};
    if(values[0]>values[1])
    {
        btSwap(span[0],span[1]);
        btSwap(values[0],values[1]);
    }
    if(values[0]>-accuracy) return(-1);
    if(values[1]<+accuracy) return(-1);
    for(int i=0;i<maxiterations;++i)
    {
        const btScalar    t=Lerp(span[0],span[1],values[0]/(values[0]-values[1]));
        const btScalar    v=fn->Eval(Lerp(a,b,t));
        if((t<=0)||(t>=1))        break;
        if(btFabs(v)<accuracy)    return(t);
        if(v<0)
        { span[0]=t;values[0]=v; }
        else
        { span[1]=t;values[1]=v; }
    }
    return(-1);
}

//
static inline btVector3        NormalizeAny(const btVector3& v)
{
    const btScalar l=v.length();
    if(l>SIMD_EPSILON)
        return(v/l);
    else
        return(btVector3(0,0,0));
}

//
static inline btDbvtVolume    VolumeOf(    const btSoftBody::Face& f,
                                     btScalar margin)
{
    const btVector3*    pts[]={    &f.m_n[0]->m_x,
        &f.m_n[1]->m_x,
        &f.m_n[2]->m_x};
    btDbvtVolume        vol=btDbvtVolume::FromPoints(pts,3);
    vol.Expand(btVector3(margin,margin,margin));
    return(vol);
}

//
static inline btVector3            CenterOf(    const btSoftBody::Face& f)
{
    return((f.m_n[0]->m_x+f.m_n[1]->m_x+f.m_n[2]->m_x)/3);
}

//
static inline btScalar            AreaOf(        const btVector3& x0,
                                       const btVector3& x1,
                                       const btVector3& x2)
{
    const btVector3    a=x1-x0;
    const btVector3    b=x2-x0;
    const btVector3    cr=cross(a,b);
    const btScalar    area=cr.length();
    return(area);
}

//
static inline btScalar        VolumeOf(    const btVector3& x0,
                                     const btVector3& x1,
                                     const btVector3& x2,
                                     const btVector3& x3)
{
    const btVector3    a=x1-x0;
    const btVector3    b=x2-x0;
    const btVector3    c=x3-x0;
    return(dot(a,cross(b,c)));
}

//
static void                    EvaluateMedium(    const btSoftBodyWorldInfo* wfi,
                                           const btVector3& x,
                                           btSoftBody::sMedium& medium)
{
    medium.m_velocity    =    btVector3(0,0,0);
    medium.m_pressure    =    0;
    medium.m_density    =    wfi->air_density;
    if(wfi->water_density>0)
    {
        const btScalar    depth=-(dot(x,wfi->water_normal)+wfi->water_offset);
        if(depth>0)
        {
            medium.m_density    =    wfi->water_density;
            medium.m_pressure    =    depth*wfi->water_density*wfi->m_gravity.length();
        }
    }
}

//
static inline void            ApplyClampedForce(    btSoftBody::Node& n,
                                              const btVector3& f,
                                              btScalar dt)
{
    const btScalar    dtim=dt*n.m_im;
    if((f*dtim).length2()>n.m_v.length2())
    {/* Clamp    */
        n.m_f-=ProjectOnAxis(n.m_v,f.normalized())/dtim;
    }
    else
    {/* Apply    */
        n.m_f+=f;
    }
}

//
static inline int        MatchEdge(    const btSoftBody::Node* a,
                                  const btSoftBody::Node* b,
                                  const btSoftBody::Node* ma,
                                  const btSoftBody::Node* mb)
{
    if((a==ma)&&(b==mb)) return(0);
    if((a==mb)&&(b==ma)) return(1);
    return(-1);
}

//
// btEigen : Extract eigen system,
// straitforward implementation of http://math.fullerton.edu/mathews/n2003/JacobiMethodMod.html
// outputs are NOT sorted.
//
struct    btEigen
{
    static int            system(btMatrix3x3& a,btMatrix3x3* vectors,btVector3* values=0)
    {
        static const int        maxiterations=16;
        static const btScalar    accuracy=(btScalar)0.0001;
        btMatrix3x3&            v=*vectors;
        int                        iterations=0;
        vectors->setIdentity();
        do    {
            int                p=0,q=1;
            if(btFabs(a[p][q])<btFabs(a[0][2])) { p=0;q=2; }
            if(btFabs(a[p][q])<btFabs(a[1][2])) { p=1;q=2; }
            if(btFabs(a[p][q])>accuracy)
            {
                const btScalar    w=(a[q][q]-a[p][p])/(2*a[p][q]);
                const btScalar    z=btFabs(w);
                const btScalar    t=w/(z*(btSqrt(1+w*w)+z));
                if(t==t)/* [WARNING] let hope that one does not get thrown aways by some compilers... */
                {
                    const btScalar    c=1/btSqrt(t*t+1);
                    const btScalar    s=c*t;
                    mulPQ(a,c,s,p,q);
                    mulTPQ(a,c,s,p,q);
                    mulPQ(v,c,s,p,q);
                } else break;
            } else break;
        } while((++iterations)<maxiterations);
        if(values)
        {
            *values=btVector3(a[0][0],a[1][1],a[2][2]);
        }
        return(iterations);
    }
private:
    static inline void    mulTPQ(btMatrix3x3& a,btScalar c,btScalar s,int p,int q)
    {
        const btScalar    m[2][3]={    {a[p][0],a[p][1],a[p][2]},
        {a[q][0],a[q][1],a[q][2]}};
        int i;

        for(i=0;i<3;++i) a[p][i]=c*m[0][i]-s*m[1][i];
        for(i=0;i<3;++i) a[q][i]=c*m[1][i]+s*m[0][i];
    }
    static inline void    mulPQ(btMatrix3x3& a,btScalar c,btScalar s,int p,int q)
    {
        const btScalar    m[2][3]={    {a[0][p],a[1][p],a[2][p]},
        {a[0][q],a[1][q],a[2][q]}};
        int i;

        for(i=0;i<3;++i) a[i][p]=c*m[0][i]-s*m[1][i];
        for(i=0;i<3;++i) a[i][q]=c*m[1][i]+s*m[0][i];
    }
};

//
// Polar decomposition,
// "Computing the Polar Decomposition with Applications", Nicholas J. Higham, 1986.
//
static inline int            PolarDecompose(    const btMatrix3x3& m,btMatrix3x3& q,btMatrix3x3& s)
{
    static const btScalar    half=(btScalar)0.5;
    static const btScalar    accuracy=(btScalar)0.0001;
    static const int        maxiterations=16;
    int                        i=0;
    btScalar                det=0;
    q    =    Mul(m,1/btVector3(m[0][0],m[1][1],m[2][2]).length());
    det    =    q.determinant();
    if(!btFuzzyZero(det))
    {
        for(;i<maxiterations;++i)
        {
            q=Mul(Add(q,Mul(q.adjoint(),1/det).transpose()),half);
            const btScalar    ndet=q.determinant();
            if(Sq(ndet-det)>accuracy) det=ndet; else break;
        }
        /* Final orthogonalization    */
        Orthogonalize(q);
        /* Compute 'S'                */
        s=q.transpose()*m;
    }
    else
    {
        q.setIdentity();
        s.setIdentity();
    }
    return(i);
}

//
// btSoftColliders
//
struct btSoftColliders
{
    //
    // ClusterBase
    //
    struct    ClusterBase : btDbvt::ICollide
    {
        btScalar            erp;
        btScalar            idt;
        btScalar            margin;
        btScalar            friction;
        btScalar            threshold;
        ClusterBase()
        {
            erp            =(btScalar)1;
            idt            =0;
            margin        =0;
            friction    =0;
            threshold    =(btScalar)0;
        }
        bool                SolveContact(    const btGjkEpaSolver2::sResults& res,
            btSoftBody::Body ba,btSoftBody::Body bb,
            btSoftBody::CJoint& joint)
        {
            if(res.distance<margin)
            {
                const btVector3        ra=res.witnesses[0]-ba.xform().getOrigin();
                const btVector3        rb=res.witnesses[1]-bb.xform().getOrigin();
                const btVector3        va=ba.velocity(ra);
                const btVector3        vb=bb.velocity(rb);
                const btVector3        vrel=va-vb;
                const btScalar        rvac=dot(vrel,res.normal);
                const btScalar        depth=res.distance-margin;
                const btVector3        iv=res.normal*rvac;
                const btVector3        fv=vrel-iv;
                joint.m_bodies[0]    =    ba;
                joint.m_bodies[1]    =    bb;
                joint.m_refs[0]        =    ra*ba.xform().getBasis();
                joint.m_refs[1]        =    rb*bb.xform().getBasis();
                joint.m_rpos[0]        =    ra;
                joint.m_rpos[1]        =    rb;
                joint.m_cfm            =    1;
                joint.m_erp            =    1;
                joint.m_life        =    0;
                joint.m_maxlife        =    0;
                joint.m_split        =    1;
                joint.m_drift        =    depth*res.normal;
                joint.m_normal        =    res.normal;
                joint.m_delete        =    false;
                joint.m_friction    =    fv.length2()<(-rvac*friction)?1:friction;
                joint.m_massmatrix    =    ImpulseMatrix(    ba.invMass(),ba.invWorldInertia(),joint.m_rpos[0],
                    bb.invMass(),bb.invWorldInertia(),joint.m_rpos[1]);
                return(true);
            }
            return(false);
        }
    };
    //
    // CollideCL_RS
    //
    struct    CollideCL_RS : ClusterBase
    {
        btSoftBody*        psb;

        btCollisionObject*    m_colObj;
        void        Process(const btDbvtNode* leaf)
        {
            btSoftBody::Cluster*        cluster=(btSoftBody::Cluster*)leaf->data;
            btSoftClusterCollisionShape    cshape(cluster);
            const btConvexShape*        rshape=(const btConvexShape*)m_colObj->getCollisionShape();
            btGjkEpaSolver2::sResults    res;
            if(btGjkEpaSolver2::SignedDistance(    &cshape,btTransform::getIdentity(),
                rshape,m_colObj->getInterpolationWorldTransform(),
                btVector3(1,0,0),res))
            {
                btSoftBody::CJoint    joint;
                if(SolveContact(res,cluster,m_colObj,joint))//prb,joint))
                {
                    btSoftBody::CJoint*    pj=new(btAlignedAlloc(sizeof(btSoftBody::CJoint),16)) btSoftBody::CJoint();
                    *pj=joint;psb->m_joints.push_back(pj);
                    if(m_colObj->isStaticOrKinematicObject())
                    {
                        pj->m_erp    *=    psb->m_cfg.kSKHR_CL;
                        pj->m_split    *=    psb->m_cfg.kSK_SPLT_CL;
                    }
                    else
                    {
                        pj->m_erp    *=    psb->m_cfg.kSRHR_CL;
                        pj->m_split    *=    psb->m_cfg.kSR_SPLT_CL;
                    }
                }
            }
        }
        void        Process(btSoftBody* ps,btCollisionObject* colOb)
        {
            psb            =    ps;
            m_colObj            =    colOb;
            idt            =    ps->m_sst.isdt;
            margin        =    m_colObj->getCollisionShape()->getMargin();
            ///Bullet rigid body uses multiply instead of minimum to determine combined friction. Some customization would be useful.
            friction    =    btMin(psb->m_cfg.kDF,m_colObj->getFriction());
            btVector3            mins;
            btVector3            maxs;

            ATTRIBUTE_ALIGNED16(btDbvtVolume)        volume;
            colOb->getCollisionShape()->getAabb(colOb->getInterpolationWorldTransform(),mins,maxs);
            volume=btDbvtVolume::FromMM(mins,maxs);
            volume.Expand(btVector3(1,1,1)*margin);
            ps->m_cdbvt.collideTV(ps->m_cdbvt.m_root,volume,*this);
        }
    };
    //
    // CollideCL_SS
    //
    struct    CollideCL_SS : ClusterBase
    {
        btSoftBody*    bodies[2];
        void        Process(const btDbvtNode* la,const btDbvtNode* lb)
        {
            btSoftBody::Cluster*        cla=(btSoftBody::Cluster*)la->data;
            btSoftBody::Cluster*        clb=(btSoftBody::Cluster*)lb->data;


            bool connected=false;
            if ((bodies[0]==bodies[1])&&(bodies[0]->m_clusterConnectivity.size()))
            {
                connected = bodies[0]->m_clusterConnectivity[cla->m_clusterIndex+bodies[0]->m_clusters.size()*clb->m_clusterIndex];
            }

            if (!connected)
            {
                btSoftClusterCollisionShape    csa(cla);
                btSoftClusterCollisionShape    csb(clb);
                btGjkEpaSolver2::sResults    res;
                if(btGjkEpaSolver2::SignedDistance(    &csa,btTransform::getIdentity(),
                    &csb,btTransform::getIdentity(),
                    cla->m_com-clb->m_com,res))
                {
                    btSoftBody::CJoint    joint;
                    if(SolveContact(res,cla,clb,joint))
                    {
                        btSoftBody::CJoint*    pj=new(btAlignedAlloc(sizeof(btSoftBody::CJoint),16)) btSoftBody::CJoint();
                        *pj=joint;bodies[0]->m_joints.push_back(pj);
                        pj->m_erp    *=    btMax(bodies[0]->m_cfg.kSSHR_CL,bodies[1]->m_cfg.kSSHR_CL);
                        pj->m_split    *=    (bodies[0]->m_cfg.kSS_SPLT_CL+bodies[1]->m_cfg.kSS_SPLT_CL)/2;
                    }
                }
            } else
            {
                static int count=0;
                count++;
                //printf("count=%d\n",count);

            }
        }
        void        Process(btSoftBody* psa,btSoftBody* psb)
        {
            idt            =    psa->m_sst.isdt;
            margin        =    (psa->getCollisionShape()->getMargin()+psb->getCollisionShape()->getMargin())/2;
            friction    =    btMin(psa->m_cfg.kDF,psb->m_cfg.kDF);
            bodies[0]    =    psa;
            bodies[1]    =    psb;
            psa->m_cdbvt.collideTT(psa->m_cdbvt.m_root,psb->m_cdbvt.m_root,*this);
        }
    };
    //
    // CollideSDF_RS
    //
    struct    CollideSDF_RS : btDbvt::ICollide
    {
        void        Process(const btDbvtNode* leaf)
        {
            btSoftBody::Node*    node=(btSoftBody::Node*)leaf->data;
            DoNode(*node);
        }
        void        DoNode(btSoftBody::Node& n) const
        {
            const btScalar            m=n.m_im>0?dynmargin:stamargin;
            btSoftBody::RContact    c;
            if(    (!n.m_battach)&&
                psb->checkContact(m_colObj1,n.m_x,m,c.m_cti))
            {
                const btScalar    ima=n.m_im;
                const btScalar    imb= m_rigidBody? m_rigidBody->getInvMass() : 0.f;
                const btScalar    ms=ima+imb;
                if(ms>0)
                {
                    const btTransform&    wtr=m_rigidBody?m_rigidBody->getInterpolationWorldTransform() : m_colObj1->getWorldTransform();
                    static const btMatrix3x3    iwiStatic(0,0,0,0,0,0,0,0,0);
                    const btMatrix3x3&    iwi=m_rigidBody?m_rigidBody->getInvInertiaTensorWorld() : iwiStatic;
                    const btVector3        ra=n.m_x-wtr.getOrigin();
                    const btVector3        va=m_rigidBody ? m_rigidBody->getVelocityInLocalPoint(ra)*psb->m_sst.sdt : btVector3(0,0,0);
                    const btVector3        vb=n.m_x-n.m_q;
                    const btVector3        vr=vb-va;
                    const btScalar        dn=dot(vr,c.m_cti.m_normal);
                    const btVector3        fv=vr-c.m_cti.m_normal*dn;
                    const btScalar        fc=psb->m_cfg.kDF*m_colObj1->getFriction();
                    c.m_node    =    &n;
                    c.m_c0        =    ImpulseMatrix(psb->m_sst.sdt,ima,imb,iwi,ra);
                    c.m_c1        =    ra;
                    c.m_c2        =    ima*psb->m_sst.sdt;
                    c.m_c3        =    fv.length2()<(btFabs(dn)*fc)?0:1-fc;
                    c.m_c4        =    m_colObj1->isStaticOrKinematicObject()?psb->m_cfg.kKHR:psb->m_cfg.kCHR;
                    psb->m_rcontacts.push_back(c);
                    if (m_rigidBody)
                        m_rigidBody->activate();
                }
            }
        }
        btSoftBody*        psb;
        btCollisionObject*    m_colObj1;
        btRigidBody*    m_rigidBody;
        btScalar        dynmargin;
        btScalar        stamargin;
    };
    //
    // CollideVF_SS
    //
    struct    CollideVF_SS : btDbvt::ICollide
    {
        void        Process(const btDbvtNode* lnode,
            const btDbvtNode* lface)
        {
            btSoftBody::Node*    node=(btSoftBody::Node*)lnode->data;
            btSoftBody::Face*    face=(btSoftBody::Face*)lface->data;
            btVector3            o=node->m_x;
            btVector3            p;
            btScalar            d=SIMD_INFINITY;
            ProjectOrigin(    face->m_n[0]->m_x-o,
                face->m_n[1]->m_x-o,
                face->m_n[2]->m_x-o,
                p,d);
            const btScalar    m=mrg+(o-node->m_q).length()*2;
            if(d<(m*m))
            {
                const btSoftBody::Node*    n[]={face->m_n[0],face->m_n[1],face->m_n[2]};
                const btVector3            w=BaryCoord(n[0]->m_x,n[1]->m_x,n[2]->m_x,p+o);
                const btScalar            ma=node->m_im;
                btScalar                mb=BaryEval(n[0]->m_im,n[1]->m_im,n[2]->m_im,w);
                if(    (n[0]->m_im<=0)||
                    (n[1]->m_im<=0)||
                    (n[2]->m_im<=0))
                {
                    mb=0;
                }
                const btScalar    ms=ma+mb;
                if(ms>0)
                {
                    btSoftBody::SContact    c;
                    c.m_normal        =    p/-btSqrt(d);
                    c.m_margin        =    m;
                    c.m_node        =    node;
                    c.m_face        =    face;
                    c.m_weights        =    w;
                    c.m_friction    =    btMax(psb[0]->m_cfg.kDF,psb[1]->m_cfg.kDF);
                    c.m_cfm[0]        =    ma/ms*psb[0]->m_cfg.kSHR;
                    c.m_cfm[1]        =    mb/ms*psb[1]->m_cfg.kSHR;
                    psb[0]->m_scontacts.push_back(c);
                }
            }
        }
        btSoftBody*        psb[2];
        btScalar        mrg;
    };
};

#endif //_BT_SOFT_BODY_INTERNALS_H
